{
 "metadata": {
  "name": "",
  "signature": "sha256:8dcad3526de13a74b2d1cc54cd9d0b71d8960ebb28a58138139e681a03d8bdf2"
 },
 "nbformat": 3,
 "nbformat_minor": 0,
 "worksheets": [
  {
   "cells": [
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "import Leap, sys, thread, time\n",
      "from Leap import CircleGesture, KeyTapGesture, ScreenTapGesture, SwipeGesture\n",
      "from IPython.html.widgets import TextWidget, ImageWidget, HTMLWidget\n",
      "from IPython.display import display\n",
      "import numpy as np\n",
      "import cv2\n",
      "from PIL import Image as PIL_Image\n",
      "from io import BytesIO\n",
      "cv2.startWindowThread()\n",
      "def img_to_png(ima, cvt=None):\n",
      "    if cvt:\n",
      "        ima = cv2.cvtColor(ima, cvt)\n",
      "    im = PIL_Image.fromarray(ima)\n",
      "    bio = BytesIO()\n",
      "    im.save(bio, format='png')\n",
      "    return bio.getvalue()"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 8
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# Initial Intrinsic Camaera Parameters of the iSight HD Camera of my MacBookAir 11\" 2012\n",
      "# the resolution is 1280x720, so if your camera is not, resize the image to be 1280x720\n",
      "cm0 = np.float32([[  1.25542912e+03,   0.00000000e+00,   6.39500000e+02],\n",
      "       [  0.00000000e+00,   1.23283056e+03,   3.59500000e+02],\n",
      "       [  0.00000000e+00,   0.00000000e+00,   1.00000000e+00]])\n",
      "dist0 = np.float32([[ 0.54520465, -1.47166739,  0.02451086, -0.02110954,  1.96975927]])"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 9
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "controller = Leap.Controller()"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 10
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "cm=None"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 11
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# Interactive calibration routine\n",
      "# touch red point and press space key, when done, press 'q' key.\n",
      "test_pos=[(0.1+(1.0*i/4)*0.8,0.1+(1.0*j/2)*0.8) for i in range(5) for j in range(3)]\n",
      "result = []\n",
      "screen_pos = []\n",
      "cap = cv2.VideoCapture(0)\n",
      "controller.set_policy_flags(Leap.Controller.POLICY_BACKGROUND_FRAMES)\n",
      "txt = TextWidget()\n",
      "txt.set_css({\"width\":\"800px\"})\n",
      "display(txt)\n",
      "hand1txt = TextWidget()\n",
      "hand1txt.set_css({\"width\":\"800px\"})\n",
      "display(hand1txt)\n",
      "#img_widget = ImageWidget(width=600)\n",
      "#display(img_widget)\n",
      "idx = 0\n",
      "while True:\n",
      "    ret, img = cap.read()\n",
      "    # img = cv2.resize(img, (960,540))\n",
      "    H, W = img.shape[:2]\n",
      "    #cv2.circle(img, (int(W*0.8), int(H*0.8)), 10, (0,0,255), -1)\n",
      "    \n",
      "    \n",
      "    #img_widget.value = img_to_png(img, cvt=cv2.COLOR_BGR2RGB)\n",
      "    frame = controller.frame()\n",
      "    keycode = cv2.waitKey(1) & 0xff\n",
      "        \n",
      "    txt.value = \"Frame id: %d, hands: %d, fingers: %d, tools: %d, gestures: %d\" % (\n",
      "              i, len(frame.hands), len(frame.fingers), len(frame.tools), len(frame.gestures()))\n",
      "    tip = None\n",
      "    if len(frame.hands)>0:\n",
      "        hand1 = frame.hands[0]\n",
      "        fingers = hand1.fingers.finger_type(1)\n",
      "        if len(fingers):\n",
      "            tip = fingers[0].tip_position\n",
      "            tip_s=\"(%5.1f, %5.1f, %5.1f)\"%(tip[0], tip[1], tip[2])\n",
      "            #cv2.circle(img, (int(400-tip[0]*2), int(H-tip[1]*2)), 10, (0,0,255), -1)\n",
      "            if cm is not None:\n",
      "                xy = cv2.projectPoints(np.float32([(tip[0], tip[1], tip[2])]), rvec[0], tvec[0], cm, dist)[0][0][0]\n",
      "                cv2.circle(img, (int(xy[0]), int(xy[1])), 10, (0,255,0), -1)\n",
      "        else:\n",
      "            tip_s=\"\"\n",
      "        hand1txt.value = \"pinch %5.2f, grab %5.2f, tip %s\"%(hand1.pinch_strength, hand1.grab_strength, tip_s)\n",
      "    else:\n",
      "        hand1txt.value = \"\"\n",
      "    \n",
      "    if  keycode == ord('q'):\n",
      "        break\n",
      "    elif keycode == ord(' '):\n",
      "        if tip:\n",
      "            screen_pos.append((int(test_pos[idx][0]*W), int(test_pos[idx][1]*H)))\n",
      "            result.append((tip[0], tip[1], tip[2]))\n",
      "            idx += 1\n",
      "            if idx >= len(test_pos):\n",
      "                idx = 0\n",
      "    cv2.circle(img, (int(test_pos[idx][0]*W), int(test_pos[idx][1]*H)), 10, (0,0,255) if tip else (255,0,0), -1)\n",
      "    img = cv2.flip(img, 1)\n",
      "    cv2.imshow('frame', img)\n",
      "\n",
      "    #time.sleep(0.3)\n",
      "cv2.destroyWindow('frame')\n",
      "cap.release()\n",
      "controller.clear_policy(Leap.Controller.POLICY_BACKGROUND_FRAMES)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 12
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "#calibrate the camera\n",
      "screen_pos2 = [(W-1-x, y) for (x,y) in screen_pos]\n",
      "retval, cm, dist, rvec, tvec = cv2.calibrateCamera(np.float32([result]), \n",
      "                                                   np.float32([screen_pos]), (W, H), cm0.copy(), dist0.copy(),\n",
      "                                                   flags=cv2.CALIB_USE_INTRINSIC_GUESS)\n",
      "print retval, rvec, tvec, cm, dist"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "10.4554619182 [array([[-0.19850488],\n",
        "       [ 0.36141292],\n",
        "       [ 3.05873363]])] [array([[ -53.55142049],\n",
        "       [ 188.49369273],\n",
        "       [ 211.8587695 ]])] [[  9.65708430e+02   0.00000000e+00   9.72616832e+02]\n",
        " [  0.00000000e+00   1.00023913e+03   5.82408425e+02]\n",
        " [  0.00000000e+00   0.00000000e+00   1.00000000e+00]] [[-0.39462292  0.48380458 -0.04993624 -0.03810031 -0.27684837]]\n"
       ]
      }
     ],
     "prompt_number": 14
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# try and test your parameter\n",
      "# if not good, input data and calibrate again \n",
      "cap = cv2.VideoCapture(0)\n",
      "controller.set_policy_flags(Leap.Controller.POLICY_BACKGROUND_FRAMES)\n",
      "txt = TextWidget()\n",
      "txt.set_css({\"width\":\"800px\"})\n",
      "display(txt)\n",
      "hand1txt = TextWidget()\n",
      "hand1txt.set_css({\"width\":\"800px\"})\n",
      "display(hand1txt)\n",
      "def to_np(v):\n",
      "    return np.float32([v[0], v[1], v[2]])\n",
      "def to_tuple(v):\n",
      "    return ([v[0], v[1], v[2]])\n",
      "while True:\n",
      "    ret, img = cap.read()\n",
      "    #img = cv2.resize(img, (960,540))\n",
      "    H, W = img.shape[:2]\n",
      "    frame = controller.frame()\n",
      "    keycode = cv2.waitKey(1) & 0xff\n",
      "        \n",
      "    txt.value = \"Frame id: %d, hands: %d, fingers: %d, tools: %d, gestures: %d\" % (\n",
      "              i, len(frame.hands), len(frame.fingers), len(frame.tools), len(frame.gestures()))\n",
      "    tip = None\n",
      "    for hand1 in frame.hands:\n",
      "        for f in hand1.fingers:\n",
      "            tp = f.tip_position\n",
      "            xy = cv2.projectPoints(np.float32([to_np(tp)]), rvec[0], tvec[0], cm, dist)[0][0][0]\n",
      "            cv2.circle(img, (int(xy[0]), int(xy[1])), 10, (0,255,0), -1)\n",
      "            for bn in range(3):\n",
      "                bone = f.bone(bn)\n",
      "                if bone.is_valid:\n",
      "                    xy1 = cv2.projectPoints(np.float32([to_np(bone.prev_joint)]), rvec[0], tvec[0], cm, dist)[0][0][0]\n",
      "                    xy2 = cv2.projectPoints(np.float32([to_np(bone.next_joint)]), rvec[0], tvec[0], cm, dist)[0][0][0]\n",
      "                    try:\n",
      "                        cv2.line(img, tuple(xy1) , tuple(xy2), (255,255,255) , 3)\n",
      "                        cv2.circle(img, tuple(xy1), 5, (255,255,128), -1)\n",
      "                        cv2.circle(img, tuple(xy2), 5, (255,255,128), -1)\n",
      "                    except:\n",
      "                        pass\n",
      "        fingers = hand1.fingers.finger_type(1)\n",
      "        for f in fingers:\n",
      "            tip = f.tip_position\n",
      "            tip_s=\"(%5.1f, %5.1f, %5.1f)\"%(tip[0], tip[1], tip[2])\n",
      "            xy = cv2.projectPoints(np.float32([(tip[0], tip[1], tip[2])]), rvec[0], tvec[0], cm, dist)[0][0][0]\n",
      "            try:\n",
      "                cv2.circle(img, tuple(xy), 10, (0,0,255), -1)\n",
      "            except:\n",
      "                pass\n",
      "        else:\n",
      "            tip_s=\"\"\n",
      "        hand1txt.value = \"pinch %5.2f, grab %5.2f, tip %s\"%(hand1.pinch_strength, hand1.grab_strength, tip_s)\n",
      "    else:\n",
      "        hand1txt.value = \"\"\n",
      "    \n",
      "    if  keycode == ord('q'):\n",
      "        break\n",
      "    img = cv2.flip(img, 1)\n",
      "    cv2.imshow('frame', img)\n",
      "\n",
      "cv2.destroyWindow('frame')\n",
      "cap.release()\n",
      "controller.clear_policy(Leap.Controller.POLICY_BACKGROUND_FRAMES)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 15
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# print the camera parameter\n",
      "print \"\"\"\n",
      "    ({r:%s, \n",
      "    t:%s, \n",
      "    k1:%s, k2:%s, k3:%s, p1:%s, p2:%s, \n",
      "    fx:%s, fy:%s, cx:%s, cy:%s})\"\"\"%(list(cv2.Rodrigues(rvec[0])[0].flatten()), \n",
      "                    list(tvec[0].flatten()),\n",
      "                    dist[0][0], dist[0][1], dist[0][4], dist[0][2], dist[0][3],\n",
      "                    cm[0][0], cm[1][1], cm[0][2],cm[1][2]\n",
      "                    )\n",
      "    \n",
      "#x,y,z = tuple(np.dot(cv2.Rodrigues(R)[0], np.float32(p))+T.flatten())\n",
      "\n",
      "#map(float, cm.flatten())\n"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "    ({r:[-0.99179590504419646, 0.087149139081190902, 0.09351957172148706, -0.035554386082314614, -0.89078095931678136, 0.45303991893538953, 0.12278749271759251, 0.44599810546243557, 0.88657144187935899], \n",
        "    t:[36.874321492170779, 273.38473786687905, 200.05315567087388], \n",
        "    k1:0.232472994886, k2:-0.13679487729, k3:-0.110213304676, p1:-0.00230327829551, p2:-0.0804344551228, \n",
        "    fx:1098.48563982, fy:969.540815635, cx:433.656347542, cy:321.222754202})\n"
       ]
      }
     ],
     "prompt_number": 11
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "project"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    }
   ],
   "metadata": {}
  }
 ]
}